#ifndef IMAGETOPLANE_HPP
#define IMAGETOPLANE_HPP

#include <opencv2/opencv.hpp>

class ImageToPlane
{
//protected:
public:

    static cv::Vec3d HeadToBase(cv::Vec3d headCoordinate, cv::Vec6d transferHeadToBase)
    {
        cv::Matx33d rm;
        cv::Rodrigues(cv::Vec3d(transferHeadToBase[3], transferHeadToBase[4], transferHeadToBase[5]), rm);
        cv::Vec3d baseCoordinate=cv::Vec3d(transferHeadToBase[0], transferHeadToBase[1], transferHeadToBase[2]) + rm*headCoordinate;
        return baseCoordinate;
    }

    static cv::Vec4d GetPlane(cv::Vec3d point[3])
    {
        cv::Vec3d normal=(point[2]-point[0]).cross(point[1]-point[0]);
        double d=-normal.ddot(point[0]);
        return cv::Vec4d(normal[0],normal[1],normal[2],d);
    }

    static cv::Vec3d LineEquation(cv::Vec2d point1, cv::Vec2d point2)
    {
        return cv::Vec3d(point1[0],point1[1],1).cross(cv::Vec3d(point2[0],point2[1],1));
    }

    static cv::Vec2d RectangleCornerToCenter(cv::Vec2d pixelCorner[4])
    {
        cv::Vec3d diagonal[2]={
            LineEquation(pixelCorner[0], pixelCorner[2]),
            LineEquation(pixelCorner[1], pixelCorner[3])
        };

        cv::Vec3d intersection=diagonal[0].cross(diagonal[1]);
        return cv::Vec2d(intersection[0]/intersection[2], intersection[1]/intersection[2]);
    }

    static bool IsInsidePolygon(cv::Vec2d point, cv::Vec2d rectangleCorner[4])
    {
        cv::Vec2d rectangleCenter=RectangleCornerToCenter(rectangleCorner);
        cv::Vec3d rectangleEdge[4]={
            LineEquation(rectangleCorner[0], rectangleCorner[1]),
            LineEquation(rectangleCorner[1], rectangleCorner[2]),
            LineEquation(rectangleCorner[2], rectangleCorner[3]),
            LineEquation(rectangleCorner[3], rectangleCorner[0])
        };
        if(rectangleEdge[0].ddot(cv::Vec3d(rectangleCenter[0],rectangleCenter[1],1))<0)
            rectangleEdge[0]=-rectangleEdge[0];
        if(rectangleEdge[1].ddot(cv::Vec3d(rectangleCenter[0],rectangleCenter[1],1))<0)
            rectangleEdge[1]=-rectangleEdge[1];
        if(rectangleEdge[2].ddot(cv::Vec3d(rectangleCenter[0],rectangleCenter[1],1))<0)
            rectangleEdge[2]=-rectangleEdge[2];
        if(rectangleEdge[3].ddot(cv::Vec3d(rectangleCenter[0],rectangleCenter[1],1))<0)
            rectangleEdge[3]=-rectangleEdge[3];

        if( rectangleEdge[0].ddot(cv::Vec3d(point[0],point[1],1))>0 &&
                rectangleEdge[1].ddot(cv::Vec3d(point[0],point[1],1))>0 &&
                rectangleEdge[2].ddot(cv::Vec3d(point[0],point[1],1))>0 &&
                rectangleEdge[3].ddot(cv::Vec3d(point[0],point[1],1))>0 )
            return true;
        return false;
    }

    static cv::Mat PolygonMask(cv::Size maskSize, cv::Vec2d rectangleCorner[4])
    {
        cv::Mat mask(maskSize,CV_8UC1);

        for(int y=0;y<mask.rows;y++)
        {
            for(int x=0;x<mask.cols;x++)
            {
                if(IsInsidePolygon(cv::Vec2d(x,y),rectangleCorner))
                    mask.at<uchar>(y,x)=255;
                else
                    mask.at<uchar>(y,x)=0;
            }
        }
        return mask;
    }


    static cv::Matx23d GetHomography2D(cv::Vec2d ptFrom[3], cv::Vec2d ptTo[3])
    {
        cv::Vec6d B(ptTo[0][0], ptTo[0][1], ptTo[1][0], ptTo[1][1], ptTo[2][0], ptTo[2][1]);
        double aptr[36]={
            ptFrom[0][0], ptFrom[0][1], 1., 0., 0., 0.,
            0., 0., 0., ptFrom[0][0], ptFrom[0][1], 1.,
            ptFrom[1][0], ptFrom[1][1], 1., 0., 0., 0.,
            0., 0., 0., ptFrom[1][0], ptFrom[1][1], 1.,
            ptFrom[2][0], ptFrom[2][1], 1., 0., 0., 0.,
            0., 0., 0., ptFrom[2][0], ptFrom[2][1], 1.
        };
        cv::Matx66d A(aptr);

        cv::Vec6d X=A.inv()*B;

        cv::Matx23d H(X[0],X[1],X[2],X[3],X[4],X[5]);

        return H;
    }


    static cv::Vec2d PerTF(cv::Vec2d p, cv::Matx33d H)
    {
        cv::Vec3d po=H*cv::Vec3d(p[0],p[1],1);
        return cv::Vec2d(po[0]/po[2],po[1]/po[2]);
    }


    static cv::Vec3d PixelCoordinateToBaseCoordinate(cv::Vec2d pixelCoordinate, cv::Matx33d H, cv::Vec4d plane)
    {
        //    cv::Vec2d baseXY=H*cv::Vec3d(pixelCoordinate[0],pixelCoordinate[1],1);
        cv::Vec2d baseXY=PerTF(pixelCoordinate, H);
        double baseZ=(plane[3]+plane[0]*baseXY[0]+plane[1]*baseXY[1])/(-plane[2]);
        return cv::Vec3d(baseXY[0],baseXY[1],baseZ);
    }


    static cv::Vec6d ComputeTransform(const cv::Vec3d ptFrom[4], const cv::Vec3d ptTo[4])
    {
        cv::Vec3d ctFrom = (ptFrom[0] + ptFrom[1] + ptFrom[2] + ptFrom[3]) / 4;
        cv::Vec3d ctTo = (ptTo[0] + ptTo[1] + ptTo[2] + ptTo[3]) / 4;
        cv::Matx33d H = (ptFrom[0] - ctFrom)*((ptTo[0] - ctTo).t());
        H += (ptFrom[1] - ctFrom)*((ptTo[1] - ctTo).t());
        H += (ptFrom[2] - ctFrom)*((ptTo[2] - ctTo).t());
        H += (ptFrom[3] - ctFrom)*((ptTo[3] - ctTo).t());
        cv::Mat U, S, Vt;
        cv::SVDecomp(H, S, U, Vt);
        cv::Mat R0 = (U*Vt).t();
        cv::Matx33d R((double*)R0.ptr());
        cv::Vec3d rxyz;
        cv::Rodrigues(R, rxyz);
        cv::Vec3d xyz = ctTo - R*ctFrom;
        return cv::Vec6d(xyz[0], xyz[1], xyz[2], rxyz[0], rxyz[1], rxyz[2]);
    }





    static void SetHeadCoordinateUnitAxis(cv::Vec3d OXYZ[4], cv::Vec3d fingerXYZ)
    {
        double invsqrt2=1/sqrt(2.0);

        OXYZ[0]=cv::Vec3d(0,0,0);
        OXYZ[1]=cv::Vec3d(1,0,0);
        OXYZ[2]=cv::Vec3d(0,1,0);
        OXYZ[3]=cv::Vec3d(0,0,1);

        OXYZ[1]=cv::Vec3d(invsqrt2,-invsqrt2,0);
        OXYZ[2]=cv::Vec3d(invsqrt2,invsqrt2,0);


        OXYZ[0]+=fingerXYZ;
        OXYZ[1]+=fingerXYZ;
        OXYZ[2]+=fingerXYZ;
        OXYZ[3]+=fingerXYZ;
    }



    static void RectangleCornerPixelCoordinateToCenterBaseCoordinateUnitAxis(cv::Vec3d OXYZ[4], cv::Vec2d pixelCorner[4], cv::Matx33d H, cv::Vec4d plane)
    {
        cv::Vec2d centerPixelCoordinate=RectangleCornerToCenter(pixelCorner);

        cv::Vec3d centerBaseCoordiante=PixelCoordinateToBaseCoordinate(centerPixelCoordinate, H, plane);

        cv::Vec3d cornerBaseCoordiante[4]={
            PixelCoordinateToBaseCoordinate(pixelCorner[0], H, plane),
            PixelCoordinateToBaseCoordinate(pixelCorner[1], H, plane),
            PixelCoordinateToBaseCoordinate(pixelCorner[2], H, plane),
            PixelCoordinateToBaseCoordinate(pixelCorner[3], H, plane)
        };

        cv::Vec3d edgeVector[4]={
            cornerBaseCoordiante[0]-cornerBaseCoordiante[1],
            cornerBaseCoordiante[1]-cornerBaseCoordiante[2],
            cornerBaseCoordiante[2]-cornerBaseCoordiante[3],
            cornerBaseCoordiante[3]-cornerBaseCoordiante[0]
        };

        double edgeLength[4]={
            cv::norm(edgeVector[0]),
            cv::norm(edgeVector[1]),
            cv::norm(edgeVector[2]),
            cv::norm(edgeVector[3])
        };

        //size_t longest=std::max_element(edgeLength, edgeLength+4)-edgeLength;
        size_t longest= 0; // see if this can fix oriantation issue

        cv::Vec3d xUnitAxis=edgeVector[longest]/edgeLength[longest];

        cv::Vec3d zUnitAxis(plane[0],plane[1],plane[2]);
        double zLength=cv::norm(zUnitAxis);

        if(zUnitAxis.ddot(cv::Vec3d(0,0,1))>0)
            zUnitAxis/=-zLength;
        else
            zUnitAxis/=zLength;

        cv::Vec3d yUnitAxis=zUnitAxis.cross(xUnitAxis);

        OXYZ[0]=centerBaseCoordiante;
        OXYZ[1]=centerBaseCoordiante+xUnitAxis;
        OXYZ[2]=centerBaseCoordiante+yUnitAxis;
        OXYZ[3]=centerBaseCoordiante+zUnitAxis;
    }


    static cv::Vec6d RectangleCornerPixelCoordinateToHeadPos(cv::Vec3d fingerXYZ, cv::Vec2d pixelCorner[4], cv::Matx33d H, cv::Vec4d plane)
    {
        cv::Vec3d OXYZBase[4];
        RectangleCornerPixelCoordinateToCenterBaseCoordinateUnitAxis(OXYZBase, pixelCorner, H, plane);
        cv::Vec3d OXYZHead[4];
        SetHeadCoordinateUnitAxis(OXYZHead, fingerXYZ);
        return ComputeTransform(OXYZHead, OXYZBase);
    }

    static void PixelCoordinateToBaseCoordinateUnitAxis(cv::Vec3d OXYZ[4],
    cv::Vec2d pixelCenter, cv::Vec3d xUnitAxis, cv::Matx33d H, cv::Vec4d plane)
    {
        cv::Vec3d centerBaseCoordiante=PixelCoordinateToBaseCoordinate(pixelCenter, H, plane);

        cv::Vec3d zUnitAxis(plane[0],plane[1],plane[2]);
        double zLength=cv::norm(zUnitAxis);

        if(zUnitAxis.ddot(cv::Vec3d(0,0,1))>0)
            zUnitAxis/=-zLength;
        else
            zUnitAxis/=zLength;

        cv::Vec3d yUnitAxis=zUnitAxis.cross(xUnitAxis);

        OXYZ[0]=centerBaseCoordiante;
        OXYZ[1]=centerBaseCoordiante+xUnitAxis;
        OXYZ[2]=centerBaseCoordiante+yUnitAxis;
        OXYZ[3]=centerBaseCoordiante+zUnitAxis;
    }


    static cv::Vec6d PixelCoordinateToHeadPos(cv::Vec3d fingerXYZ, cv::Vec2d pixelCenter, cv::Vec3d xUnitAxis, cv::Matx33d H, cv::Vec4d plane)
    {
        cv::Vec3d OXYZBase[4];
        PixelCoordinateToBaseCoordinateUnitAxis(OXYZBase, pixelCenter, xUnitAxis, H, plane);
        cv::Vec3d OXYZHead[4];
        SetHeadCoordinateUnitAxis(OXYZHead, fingerXYZ);
        return ComputeTransform(OXYZHead, OXYZBase);
    }


public:
    cv::Vec4d plane;
    cv::Matx33d H;

    cv::Mat mask;

    ImageToPlane()
    {

    }

    void Calibrate(cv::Vec3d fingerXYZ, cv::Vec6d headPos[4], cv::Vec2d referencePixel[4], cv::Size maskSize, bool showDetail=true)
    {
        cv::Vec3d referenceBase[4]={
            HeadToBase(fingerXYZ, headPos[0]),
            HeadToBase(fingerXYZ, headPos[1]),
            HeadToBase(fingerXYZ, headPos[2]),
            HeadToBase(fingerXYZ, headPos[3])
        };

        plane=GetPlane(referenceBase);

        cv::Vec2d referenceBaseXY[4]={
            cv::Vec2d(referenceBase[0][0],referenceBase[0][1]),
            cv::Vec2d(referenceBase[1][0],referenceBase[1][1]),
            cv::Vec2d(referenceBase[2][0],referenceBase[2][1]),
            cv::Vec2d(referenceBase[3][0],referenceBase[3][1])
        };

        cv::Point2f referencePixelp[4]={
            cv::Point2f(referencePixel[0][0], referencePixel[0][1]),
            cv::Point2f(referencePixel[1][0], referencePixel[1][1]),
            cv::Point2f(referencePixel[2][0], referencePixel[2][1]),
            cv::Point2f(referencePixel[3][0], referencePixel[3][1])
        };

        cv::Point2f referenceBaseXYp[4]={
            cv::Point2f(referenceBaseXY[0][0], referenceBaseXY[0][1]),
            cv::Point2f(referenceBaseXY[1][0], referenceBaseXY[1][1]),
            cv::Point2f(referenceBaseXY[2][0], referenceBaseXY[2][1]),
            cv::Point2f(referenceBaseXY[3][0], referenceBaseXY[3][1])
        };

        cv::Mat HH=cv::getPerspectiveTransform(referencePixelp, referenceBaseXYp);

        H=cv::Matx33d(HH);

        if(maskSize.area()>0)
            mask=PolygonMask(maskSize,referencePixel);
        else
            mask.release();

        if(showDetail)
        {
            std::cout<<referenceBase[0]<<"\n"
                                      <<referenceBase[1]<<"\n"
                                     <<referenceBase[2]<<"\n"
                                    <<referenceBase[3]<<"\n"
                                   <<std::flush;


            std::cout<<"plane="<<plane<<"\n"<<std::flush;

            std::cout<<"H="<<H<<"\n"<<std::flush;

            std::cout<<plane.ddot(cv::Vec4d(referenceBase[0][0],referenceBase[0][1],referenceBase[0][2],1))<<"\n";
            std::cout<<plane.ddot(cv::Vec4d(referenceBase[1][0],referenceBase[1][1],referenceBase[1][2],1))<<"\n";
            std::cout<<plane.ddot(cv::Vec4d(referenceBase[2][0],referenceBase[2][1],referenceBase[2][2],1))<<"\n";
            std::cout<<plane.ddot(cv::Vec4d(referenceBase[3][0],referenceBase[3][1],referenceBase[3][2],1))<<"\n";



            //    std::cout<<cv::Vec2d(referenceBase[0][0],referenceBase[0][1])-H*cv::Vec3d(referencePixel[0][0], referencePixel[0][1], 1)<<"\n";
            //    std::cout<<cv::Vec2d(referenceBase[1][0],referenceBase[1][1])-H*cv::Vec3d(referencePixel[1][0], referencePixel[1][1], 1)<<"\n";
            //    std::cout<<cv::Vec2d(referenceBase[2][0],referenceBase[2][1])-H*cv::Vec3d(referencePixel[2][0], referencePixel[2][1], 1)<<"\n";
            //    std::cout<<cv::Vec2d(referenceBase[3][0],referenceBase[3][1])-H*cv::Vec3d(referencePixel[3][0], referencePixel[3][1], 1)<<"\n";

            std::cout<<referenceBaseXY[0]-PerTF(referencePixel[0],H)<<"\n";
            std::cout<<referenceBaseXY[1]-PerTF(referencePixel[1],H)<<"\n";
            std::cout<<referenceBaseXY[2]-PerTF(referencePixel[2],H)<<"\n";
            std::cout<<referenceBaseXY[3]-PerTF(referencePixel[3],H)<<"\n";


            if(maskSize.area()>0)
            {
                cv::imshow("mask",mask);
                cv::waitKey(1);
            }
        }
    }


    cv::Vec6d RectangleCornerPixelCoordinateToHeadPos(cv::Vec3d fingerXYZ, cv::Vec2d pixelCorner[4])
    {
        return RectangleCornerPixelCoordinateToHeadPos(fingerXYZ, pixelCorner, H, plane);
    }


    cv::Vec6d PixelCoordinateToHeadPos(cv::Vec3d fingerXYZ, cv::Vec2d pixelCenter, cv::Vec3d xUnitAxis=cv::Vec3d(1,0,0))
    {
        return PixelCoordinateToHeadPos(fingerXYZ, pixelCenter, xUnitAxis, H, plane);
    }


    cv::Vec6d PixelVectorToHeadPos(cv::Vec3d fingerXYZ, cv::Vec3d pixelVector)
    {
        cv::Vec2d pixelCenter(pixelVector[0], pixelVector[1]);

        cv::Vec2d pixelXUnitAxis(cos(pixelVector[2]),sin(pixelVector[2]));

         cv::Vec2d pixelYUnitAxis(cos(pixelVector[2]+CV_PI/2),sin(pixelVector[2]+CV_PI/2));

         cv::Vec2d pixelCorner[4]={
             pixelCenter+2*pixelXUnitAxis+pixelYUnitAxis,
             pixelCenter-2*pixelXUnitAxis+pixelYUnitAxis,
             pixelCenter-2*pixelXUnitAxis-pixelYUnitAxis,
             pixelCenter+2*pixelXUnitAxis-pixelYUnitAxis
         };

        return RectangleCornerPixelCoordinateToHeadPos(fingerXYZ, pixelCorner);
    }


    static bool Save(const cv::Mat &m, std::string fp)
    {
        cv::FileStorage fs(fp, CV_STORAGE_WRITE);
        if (fs.isOpened())
        {
            fs << "m" << m;
            fs.release();
            return true;
        }
        std::cout << "Error: can not save result\n";
        return false;
    }

    static bool Load(cv::Mat &m, std::string fp)
    {
        cv::FileStorage fs(fp, CV_STORAGE_READ);
        if (fs.isOpened())
        {
            fs["m"] >> m;
            fs.release();
            return true;
        }
        std::cout << "Failed to open file " << fp << '\n';
        return false;
    }

    void test()
    {
        cv::Vec6d headPosBase[4]={
            cv::Vec6d(0.367232,-0.101782,0.233713,2.85595,1.27128,0.0333029),
            cv::Vec6d(0.361877,0.0591402,0.231041,2.85583,1.2713,0.0333305),
            cv::Vec6d(0.462264,0.0594238,0.23545,2.85592,1.2712,0.0335398),
            cv::Vec6d(0.466046,-0.100316,0.237735,2.85584,1.27135,0.0334424)
        };


        cv::Mat chessboard;
        Load(chessboard, "chess.yml");

        cv::Vec3d chessboardCorner[4]={
            chessboard.at<cv::Vec3d>(chessboard.rows-1,0),
            chessboard.at<cv::Vec3d>(0,0),
            chessboard.at<cv::Vec3d>(0,chessboard.cols-1),
            chessboard.at<cv::Vec3d>(chessboard.rows-1,chessboard.cols-1)
        };


        cv::Vec2d referencePixel[4]={
            cv::Vec2d(chessboardCorner[0][0],chessboardCorner[0][1]),
            cv::Vec2d(chessboardCorner[1][0],chessboardCorner[1][1]),
            cv::Vec2d(chessboardCorner[2][0],chessboardCorner[2][1]),
            cv::Vec2d(chessboardCorner[3][0],chessboardCorner[3][1])
        };


        Calibrate(cv::Vec3d(0,0,0.21), headPosBase, referencePixel, cv::Size(), true);
    }

};

#endif // IMAGETOPLANE_HPP

